{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import argparse\n",
    "import time\n",
    "import msgpack\n",
    "from enum import Enum, auto\n",
    "\n",
    "import numpy as np\n",
    "\n",
    "from planning_utils import a_star, heuristic, create_grid\n",
    "from udacidrone import Drone\n",
    "from udacidrone.connection import MavlinkConnection\n",
    "from udacidrone.messaging import MsgID\n",
    "from udacidrone.frame_utils import global_to_local\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class States(Enum):\n",
    "    MANUAL = auto()\n",
    "    ARMING = auto()\n",
    "    TAKEOFF = auto()\n",
    "    WAYPOINT = auto()\n",
    "    LANDING = auto()\n",
    "    DISARMING = auto()\n",
    "    PLANNING = auto()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "class MotionPlanning(Drone):\n",
    "\n",
    "    def __init__(self, connection):\n",
    "        super().__init__(connection)\n",
    "\n",
    "        self.target_position = np.array([0.0, 0.0, 0.0])\n",
    "        self.waypoints = []\n",
    "        self.in_mission = True\n",
    "        self.check_state = {}\n",
    "\n",
    "        # initial state\n",
    "        self.flight_state = States.MANUAL\n",
    "\n",
    "        # register all your callbacks here\n",
    "        self.register_callback(MsgID.LOCAL_POSITION, self.local_position_callback)\n",
    "        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)\n",
    "        self.register_callback(MsgID.STATE, self.state_callback)\n",
    "\n",
    "    def local_position_callback(self):\n",
    "        if self.flight_state == States.TAKEOFF:\n",
    "            if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:\n",
    "                self.waypoint_transition()\n",
    "        elif self.flight_state == States.WAYPOINT:\n",
    "            if np.linalg.norm(self.target_position[0:2] - self.local_position[0:2]) < 1.0:\n",
    "                if len(self.waypoints) > 0:\n",
    "                    self.waypoint_transition()\n",
    "                else:\n",
    "                    if np.linalg.norm(self.local_velocity[0:2]) < 1.0:\n",
    "                        self.landing_transition()\n",
    "\n",
    "    def velocity_callback(self):\n",
    "        if self.flight_state == States.LANDING:\n",
    "            if self.global_position[2] - self.global_home[2] < 0.1:\n",
    "                if abs(self.local_position[2]) < 0.01:\n",
    "                    self.disarming_transition()\n",
    "\n",
    "    def state_callback(self):\n",
    "        if self.in_mission:\n",
    "            if self.flight_state == States.MANUAL:\n",
    "                self.arming_transition()\n",
    "            elif self.flight_state == States.ARMING:\n",
    "                if self.armed:\n",
    "                    self.plan_path()\n",
    "            elif self.flight_state == States.PLANNING:\n",
    "                self.takeoff_transition()\n",
    "            elif self.flight_state == States.DISARMING:\n",
    "                if ~self.armed & ~self.guided:\n",
    "                    self.manual_transition()\n",
    "\n",
    "    def arming_transition(self):\n",
    "        self.flight_state = States.ARMING\n",
    "        print(\"arming transition\")\n",
    "        self.arm()\n",
    "        self.take_control()\n",
    "\n",
    "    def takeoff_transition(self):\n",
    "        self.flight_state = States.TAKEOFF\n",
    "        print(\"takeoff transition\")\n",
    "        self.takeoff(self.target_position[2])\n",
    "\n",
    "    def waypoint_transition(self):\n",
    "        self.flight_state = States.WAYPOINT\n",
    "        print(\"waypoint transition\")\n",
    "        self.target_position = self.waypoints.pop(0)\n",
    "        print('target position', self.target_position)\n",
    "        self.cmd_position(self.target_position[0], self.target_position[1], self.target_position[2], self.target_position[3])\n",
    "\n",
    "    def landing_transition(self):\n",
    "        self.flight_state = States.LANDING\n",
    "        print(\"landing transition\")\n",
    "        self.land()\n",
    "\n",
    "    def disarming_transition(self):\n",
    "        self.flight_state = States.DISARMING\n",
    "        print(\"disarm transition\")\n",
    "        self.disarm()\n",
    "        self.release_control()\n",
    "\n",
    "    def manual_transition(self):\n",
    "        self.flight_state = States.MANUAL\n",
    "        print(\"manual transition\")\n",
    "        self.stop()\n",
    "        self.in_mission = False\n",
    "\n",
    "    def send_waypoints(self):\n",
    "        print(\"Sending waypoints to simulator ...\")\n",
    "        data = msgpack.dumps(self.waypoints)\n",
    "        self.connection._master.write(data)\n",
    "\n",
    "    def plan_path(self):\n",
    "        self.flight_state = States.PLANNING\n",
    "        print(\"Searching for a path ...\")\n",
    "        TARGET_ALTITUDE = 5\n",
    "        SAFETY_DISTANCE = 5\n",
    "\n",
    "        self.target_position[2] = TARGET_ALTITUDE\n",
    "\n",
    "        # TODO: read lat0, lon0 from colliders into floating point values\n",
    "        \n",
    "        # TODO: set home position to (lon0, lat0, 0)\n",
    "\n",
    "        # TODO: retrieve current global position\n",
    " \n",
    "        # TODO: convert to current local position using global_to_local()\n",
    "        \n",
    "        print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,\n",
    "                                                                         self.local_position))\n",
    "        # Read in obstacle map\n",
    "        data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)\n",
    "        \n",
    "        # Define a grid for a particular altitude and safety margin around obstacles\n",
    "        grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)\n",
    "        print(\"North offset = {0}, east offset = {1}\".format(north_offset, east_offset))\n",
    "        # Define starting point on the grid (this is just grid center)\n",
    "        grid_start = (-north_offset, -east_offset)\n",
    "        # TODO: convert start position to current position rather than map center\n",
    "        \n",
    "        # Set goal as some arbitrary position on the grid\n",
    "        grid_goal = (-north_offset + 10, -east_offset + 10)\n",
    "        # TODO: adapt to set goal as latitude / longitude position and convert\n",
    "\n",
    "        # Run A* to find a path from start to goal\n",
    "        # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation\n",
    "        # or move to a different search space such as a graph (not done here)\n",
    "        print('Local Start and Goal: ', grid_start, grid_goal)\n",
    "        path, _ = a_star(grid, heuristic, grid_start, grid_goal)\n",
    "        \n",
    "        # TODO: prune path to minimize number of waypoints\n",
    "        # TODO (if you're feeling ambitious): Try a different approach altogether!\n",
    "\n",
    "        # Convert path to waypoints\n",
    "        waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]\n",
    "        # Set self.waypoints\n",
    "        self.waypoints = waypoints\n",
    "        # TODO: send waypoints to sim\n",
    "        self.send_waypoints()\n",
    "\n",
    "    def start(self):\n",
    "        self.start_log(\"Logs\", \"NavLog.txt\")\n",
    "\n",
    "        print(\"starting connection\")\n",
    "        self.connection.start()\n",
    "\n",
    "        # Only required if they do threaded\n",
    "        # while self.in_mission:\n",
    "        #    pass\n",
    "\n",
    "        self.stop_log()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#if __name__ == \"__main__\":\n",
    "parser = argparse.ArgumentParser()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "parser.add_argument('--port', type=int, default=5760, help='Port number')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "parser.add_argument('--host', type=str, default='127.0.0.1', help=\"host address, i.e. '127.0.0.1'\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "args = parser.parse_args()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%tb"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "conn = MavlinkConnection('tcp:127.0.0.1:5760', timeout=60)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Logs/TLog.txt\n"
     ]
    }
   ],
   "source": [
    "drone = MotionPlanning(conn)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "time.sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Logs/NavLog.txt\n",
      "starting connection\n",
      "arming transition\n",
      "Searching for a path ...\n",
      "global home [-122.39745   37.79248    0.     ], position [-122.39745     37.7924816    0.165    ], local position [ 0.18457605 -0.00058904 -0.16552639]\n",
      "North offset = -316, east offset = -445\n",
      "Local Start and Goal:  (316, 445) (326, 455)\n",
      "Found a path.\n",
      "Sending waypoints to simulator ...\n",
      "takeoff transition\n",
      "waypoint transition\n",
      "target position [0, 0, 5, 0]\n",
      "waypoint transition\n",
      "target position [0, 1, 5, 0]\n",
      "waypoint transition\n",
      "target position [1, 1, 5, 0]\n",
      "waypoint transition\n",
      "target position [1, 2, 5, 0]\n",
      "waypoint transition\n",
      "target position [2, 2, 5, 0]\n",
      "waypoint transition\n",
      "target position [2, 3, 5, 0]\n",
      "waypoint transition\n",
      "target position [3, 3, 5, 0]\n",
      "waypoint transition\n",
      "target position [3, 4, 5, 0]\n",
      "waypoint transition\n",
      "target position [4, 4, 5, 0]\n",
      "waypoint transition\n",
      "target position [4, 5, 5, 0]\n",
      "waypoint transition\n",
      "target position [5, 5, 5, 0]\n",
      "waypoint transition\n",
      "target position [5, 6, 5, 0]\n",
      "waypoint transition\n",
      "target position [6, 6, 5, 0]\n",
      "waypoint transition\n",
      "target position [6, 7, 5, 0]\n",
      "waypoint transition\n",
      "target position [7, 7, 5, 0]\n",
      "waypoint transition\n",
      "target position [7, 8, 5, 0]\n",
      "waypoint transition\n",
      "target position [8, 8, 5, 0]\n",
      "waypoint transition\n",
      "target position [8, 9, 5, 0]\n",
      "waypoint transition\n",
      "target position [9, 9, 5, 0]\n",
      "waypoint transition\n",
      "target position [9, 10, 5, 0]\n",
      "waypoint transition\n",
      "target position [10, 10, 5, 0]\n",
      "landing transition\n",
      "disarm transition\n",
      "manual transition\n",
      "Closing connection ...\n"
     ]
    }
   ],
   "source": [
    "drone.start()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "VPython",
   "language": "python",
   "name": "vpython"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
